#include <stdio.h>
#include <termios.h>
#include "gps.h"

int main()
{
	gps_session_t session;
	gps_data_t data;
	ref_frame_t ref;
	vector3_t vec;
	char sentence[85];
	bool first = true;
	
	data.quality = GPS_QUALITY_NO_FIX;
	data.info = GPS_INFO_NONE;
	
	gps_open_session(B19200, &session);
	
	while (true) {
		if (gps_read_sentence(&session, sentence, 84) > 0) {
			int type = gps_parse_sentence(sentence, &data);
			switch (type) {
				case GPS_INFO_NONE:
					printf("Received invalid GPS data (no fix).\n");
					break;
				case GPS_INFO_POSITION:
					if (first) {
						gps_create_ref_frame(&data.coord, &ref);
						printf("Created new reference frame at:\n");
						printf("  Geodetic: %.8f, %.8f, %.8f\n", fixed_to_double(ref.geodetic.lat), fixed_to_double(ref.geodetic.lon), fixed_to_double(ref.geodetic.alt));
						printf("  ECEF: %.8f, %.8f, %.8f\n", fixed_to_double(ref.ecef.x), fixed_to_double(ref.ecef.y), fixed_to_double(ref.ecef.z));
						first = false;
					} else {
						gps_geodetic_to_enu(&ref, &data.coord, &vec);
						printf("Position:\n");
						printf("  Geodetic: %.8f, %.8f, %.8f\n", fixed_to_double(data.coord.lat), fixed_to_double(data.coord.lon), fixed_to_double(data.coord.alt));
						printf("  Local ENU: %.8f, %.8f, %.8f\n", fixed_to_double(vec.x), fixed_to_double(vec.y), fixed_to_double(vec.z));
					}
					break;
				case GPS_INFO_VELOCITY:
					printf("Velocity: %.8f, %.8f, %.8f\n", fixed_to_double(data.velocity.x), fixed_to_double(data.velocity.y), fixed_to_double(data.velocity.z));
					break;
			}

		}
	}
	
	gps_close_session(&session);
	
	return 0;
}
